\documentclass{article}

\usepackage{amsmath}
\usepackage{amssymb}

\begin{document}



\section{Pitch Yaw Roll}
To attach meaning to orientation angles we will work in cartesian coordinates with right-handed basis ${x, y, z}$ with the $z$ axis pointing vertically upwards. 
Let the airplane be located at the center of the coordinate system, with the top of the plane facing the upward direction of the $z$ axis, and its front pointing along the positive $x$ axis. 
The pitch ($\theta$), yaw ($\psi$) and roll ($\phi$) angles are defined as (euler) rotation angles between two coordinate systems: the system aligned with the coordinates of the plane, and the stationary "lab" reference frame.
The two coordinate systems coincide for $\theta = \psi = \phi = 0$.
It is conceptually easiest to define the three angles individually, with the remaining two set to zero. 
In this way, pitch ($\theta$) is the rotation around the $y$ axis, corresponding to the inclination of the front of the plane, yaw ($\psi$) is the rotation around the $z$ axis, and roll ($\phi$) around the $x$ axis along the axis of the plane. 
Since rotations do not commute, there is an ambiguity in which the three rotations are performed, so a preferred order needs to be fixed.

{\bf Definition. } Let $\vec v$ be a vector. 
Let $v^i$ be coordinates of $\vec v$ in the laboratory coordinate frame. 
Let $\bar v^i$ be coordinates of $\vec v$ in the plane coordinate frame. 
Then $v^i = R_x(\phi)^i_j\, R_y(\theta)^j_k\, R_z(\psi)^k_l\, \bar v^l$, where the $R$ matrices are right-handed rotations around the appropriate axis:
\begin{equation}
R_x(\alpha) = 
  \begin{pmatrix}
    1           &  0           & 0 \\
    0           &  \cos \alpha & -\sin \alpha \\
    0           &  \sin \alpha &  \cos \alpha 
  \end{pmatrix}
R_y(\alpha) = 
  \begin{pmatrix}
    \cos \alpha &  0           & \sin \alpha \\
    0           &  1           & 0 \\
   -\sin \alpha &  0           & \cos \alpha
  \end{pmatrix}
R_z(\alpha) = 
  \begin{pmatrix}
    \cos \alpha & -\sin \alpha & 0 \\
    \sin \alpha &  \cos \alpha & 0 \\
    0           &  0           & 1
  \end{pmatrix}
\end{equation}

In other words, successively pre-multiplying the vector $v$ in the plane coordinate frame by the yaw, pitch and roll rotation matrices yields the laboratory coordinate frame description. 

% The rotations are understood as passive coordinate transformations, and according to the above convention, an orientation angle $\phi$, $\theta$ or $\psi$ is positive if its corresponding rotation is right-handed. 




\section{Quaternions et al}
A unit quaternion is used to represent the orientation of a vector in space. 
We identify a vector $\vec v \in \mathbb{R}^3$, $\vec v = (v_x,\, v_y,\, v_z)$ as a pure imaginary quaterion $\mathbf{q} = (0,\, v_x,\, v_y,\, v_z)$.
We express such $\mathbf{q}$ as $\mathbf{q} = (0, \vec v)$.
We also refer to the imaginary part of a quaternion as the ``spatial part''. 

Expressed in the Hamilton basis $\{1,\, i,\, j,\, k\}$, the quaternion multiplication becomes:
\begin{equation}
  \mathbf{x} \cdot \mathbf{y} = 
  \begin{pmatrix}
    x_0 \\ 
    x_1 \\ 
    x_2 \\ 
    x_3 
  \end{pmatrix}  
  \cdot 
  \begin{pmatrix}
    y_0 \\ 
    y_1 \\ 
    y_2 \\ 
    y_3 
  \end{pmatrix}  
  = 
  \begin{pmatrix}
    x_0 y_0 - x_1 y_1 - x_2 y_2 - x_3 y_3 \\
    x_0 y_1 + x_1 y_0 + x_2 y_3 - x_3 y_2 \\ 
    x_0 y_2 - x_1 y_3 + x_2 y_0 + x_3 y_1 \\
    x_0 y_3 + x_1 y_2 - x_2 y_2 + x_3 y_0 
  \end{pmatrix}
\end{equation}


Quaternion conjugation flips the sign on the spatial part:
\begin{equation}
  \mathbf{q}^* = (q_w,\, q_x,\, q_y,\, q_z)^* = (q_w,\, -q_x,\, -q_y,\, -q_z)
\end{equation}

Quaternion norm is defined as:
\begin{equation}
  |\mathbf{q}|^2 = |(q_w,\, q_x,\, q_y,\, q_z)|^2 = q_w^2 + q_x^2 + q_y^2 + q_z^2.
\end{equation}
One can easily verify that $\mathbf{q} \cdot \mathbf{q}^* = \mathbf{q}^* \cdot \mathbf{q} = (|\mathbf{q}|^2, 0, 0, 0)$.

Let $\mathbf{q}$, $\mathbf{p}$ be two quaternions. Conjugation of $\mathbf{p}$ by $\mathbf{q}$ is defined as $\mathbf{q} \cdot \mathbf{p} \cdot \mathbf{q}^*$. One can verify immediately that if $\mathbf{q} = (1, 0, 0, 0)$, then $\mathbf{q} \cdot \mathbf{p} \cdot \mathbf{q}^* = \mathbf{p}$.

A unit quaternion $|\mathbf{q}| = 1$ can be represented as $\mathbf{q} = (\cos \alpha,\, \vec q\, \sin \alpha)$ where $\vec q$ is a unit vector in $\mathbb{R}^3$, for then $|\mathbf{q}|^2 = \cos^2 \alpha + |\vec q|^2 \sin^2 \alpha = \cos^2 \alpha + \sin^2 \alpha = 1$.


{\bf Theorem.} Let $\vec v \in \mathbb{R}^3$ be a vector, and $\mathbf{q} = (\cos \tfrac{\theta}{2},\, \sin \tfrac{\theta}{2} \vec q)$ a unit quaternion, where $\vec q = (\cos \beta_x,\, \cos \beta_y,\, \cos \beta_z)$ is the imaginary (``spatial'') part of $\mathbf{q}$. 
Conjugation of $\vec v$ by $\mathbf{q}$ is equivalent to rotation of the vector $\vec v$ through an angle $\theta$ about $\vec q$, defined by its directional cosines $\cos \beta_i$, as the axis of rotation.

In fact, we can compute in components and represent quaternion conjugation of $\mathbf{v} = \left( 0,\, v_x,\, v_y,\, v_z \right)$ by $\mathbf{q} = \left(q_0,\, q_1,\, q_2,\, q_3 \right)$ as an action of a linear operator $A$ on $\mathbf{v}$: 
\begin{equation}
  \mathbf{q} \cdot \mathbf{q}_v \cdot \mathbf{q}^* = 
  \begin{pmatrix}
    1                             & 0                             & 0                             & 0                     \\ 
    0                             & q_0^2 + q_1^2 - q_2^2 - q_3^2 & 2 q_1 q_2 - 2 q_0 q_3           & 2 q_0 q_2 + 2 q_1 q_3 \\ 
    0                             & 2 q_1 q_2 + 2 q_0 q_3         & q_0^2 - q_1^2 + q_2^2 - q_3^2 & -2 q_0 q_1 + 2 q_2 q_3 \\
    0                             & -2 q_0 q_2 + 2 q_1 q_3        & 2 q_0 q_1 + 2 q_2 q_3         & q_0^2 - q_1^2 - q_2^2 + q_3^2 
  \end{pmatrix}
  \cdot 
  \begin{pmatrix}
    0 \\ 
    v_x \\
    v_y \\
    v_z 
  \end{pmatrix}
\end{equation}

The resulting operator is orthogonal when $\mathbf{q}$ is normalized to one. 
In particular, the "spatial" part (indices 1 through 3) can be identified with a spatial rotation of the vector $\vec v$.
One can quickly verify that the rotation matrices corresponding to conjugation by $\mathbf{q} = (\cos \tfrac{\theta}{2},\, \sin \tfrac{\theta}{2}\, \vec q)$, with $\vec q = (\cos \beta_x,\, \cos \beta_y,\, \cos \beta_z)$ for appropriate choices of $\beta_i$ are precisely the rotation matrices $R_i(\theta)$ (setting all $\beta_i$ but the $k$-th to zero for a rotation around the $k$ axis).

Recall that any rotation matrices we consider as giving rise to a passive coordinate transformation, so that we interpret the resulting rotations as right-handed (counter-clockwise) for positive values of $\theta$.

This geometrical equivalence allows us to use unit quaternions to encode coordinate transformations through given pitch, yaw and roll angles. 
In order to express the components of a unit quaternion $\mathbf{q}(\phi,\,\theta,\,\psi)$ as a function of the three euler angles, we demand that the effect of conjugation of a vector $\vec v$ through $\mathbf{q}(\theta,\,\psi,\,\phi)$ on its spatial part is equivalent to a coordinate transformation through $R_x(\phi)\, R_y(\theta)\,R_z(\psi)$:
\begin{equation}
  \mathbf{q}(\phi,\,\theta,\,\psi) \cdot \vec v \cdot \mathbf{q}(\phi,\,\theta,\,\psi)^* = R_x(\phi)\, R_y(\theta)\,R_z(\psi)\,\vec v
\end{equation}

Using the interpretation of quaternion conjugation as a rotation through an axis, we conveniently set $\mathbf{q}_\phi = (\cos \tfrac{\phi}{2}, \sin \tfrac{\phi}{2}, 0, 0)$, $\mathbf{q}_\theta = (\cos \tfrac{\theta}{2}, 0, \sin \tfrac{\theta}{2},  0)$ and $\mathbf{q}_\psi = (\cos \tfrac{\psi}{2}, 0, 0, \sin \tfrac{\phi}{2})$ and conclude that we must have $\mathbf{q} = \mathbf{q}_\phi \cdot \mathbf{q}_\theta \cdot \mathbf{q}_\psi$. 
In components: (roll pitch yaw)
\begin{equation}
\begin{split}
  \mathbf{q}(\theta,\,\psi,\,\phi) = (
    &\cos \tfrac{\phi}{2}\, \cos \tfrac{\theta}{2}\, \cos \tfrac{\psi}{2}  -  \sin \tfrac{\phi}{2}\, \sin \tfrac{\theta}{2}\, \sin \tfrac{\psi}{2}, \\
    &\sin \tfrac{\phi}{2}\, \cos \tfrac{\theta}{2}\, \cos \tfrac{\psi}{2}  +  \cos \tfrac{\phi}{2}\, \sin \tfrac{\theta}{2}\, \sin \tfrac{\psi}{2}, \\
    &\cos \tfrac{\phi}{2}\, \sin \tfrac{\theta}{2}\, \cos \tfrac{\psi}{2}  -  \sin \tfrac{\phi}{2}\, \cos \tfrac{\theta}{2}\, \sin \tfrac{\psi}{2}, \\
    &\cos \tfrac{\phi}{2}\, \cos \tfrac{\theta}{2}\, \sin \tfrac{\psi}{2}  +  \sin \tfrac{\phi}{2}\, \sin \tfrac{\theta}{2}\, \cos \tfrac{\psi}{2}\, )
\end{split}
\end{equation}

This algebraic correspondence in turn allows us to write the inverse relations.  
Given an arbitrary unit quaternion $\mathbf{q} = (q_0,\, q_1,\, q_2,\, q_3)$, one should be able to determine the corresponding roll, pitch and yaw angles ($\phi,\, \theta,\, \psi$) given any three out of 4 coordinates of $q$, as the fourth one is constrained by $|\mathbf{q}| = 1$. 
However, we should keep in mind the desired application of quaternions to real-world signals that tend to be noisy. 
We should seek expressions that give equal importance to all four components of $\mathbf{q}$, so that error analysis can be carried out to faithfully propagate the measurement uncertainties. 
The desired (albeit not unique) inverse relationships are:
\begin{equation}
\begin{split}
  \tan \phi &= 2\,\frac{q_0 q_1 - q_2 q_3}{q_0^2 - q_1^2 - q_2^2 + q_3^2} \\
  \sin \theta &= 2\, \left( q_0 q_2 + q_3 q_1 \right) \\
  \tan \psi &= 2\,\frac{q_0 q_3 - q_1 q_2}{q_0^2 + q_1^2 - q_2^2 - q_3^2} \\
\end{split}
\end{equation}


If the components of $\mathbf{q}$ are known to be measured (or calculated) with generally different errors $\sigma_i$, propagating the errors to $\phi$, $\theta$ and $\psi$ in the standard way as $\sigma_\alpha^2 = \sum_{i=0}^3 \big(\tfrac{\partial \alpha}{\partial q_i}\big)^2\, \sigma_i^2$ yields awful expressions. 
Nevertheless, if  $\sigma_0 = \sigma_1 = \sigma_2 = \sigma_3$, not only do the expressions simplify remarkably, but the propagated errors are equal for all three angles: 
\begin{equation}
  \sigma_\phi^2 = \sigma_\theta^2 = \sigma_\psi^2 = \frac{4}{\left(1 - 2 q_0 q_2 - 2 q_1 q_3 \right) \left(1 + 2 q_0 q_2 + 2 q_1 q_3 \right)}\, \sigma^2 = \frac{4}{\cos^2 \theta}\, \sigma^2
\end{equation}
The divergent $\tfrac{1}{\cos^2 \theta}$ behaviour of the error near $\theta = \tfrac{\pi}{2}$ is an imprint of the $\emph{gimbal lock}$ -- redundancy of the euler angles description at that point.

In applications where only approximate knowledge of the variance of the roll, pitch and yaw angles is required, or where the individual errors on the quaternion components $\sigma_i$ do not change substantially from a common mean, it may be practical to compute the average $\sigma$ and use the above result as a zeroth order estimate of the angle error.



\end{document}
